//#############################################################################

//! \file   \libraries\observers\speedcalc\source\speed_Observer.c
//! \brief  Portable C fixed point code.  These functions define the
//!         speed observer in generic form for Motor Control
//!

#include "HeaderFiles.h"    // Include speed observation definitions
#include "speedcalc.h"

// **************************************************************************
// the defines

// **************************************************************************
// the globals

SPDCALC_Obj speedpll[2];

// **************************************************************************
// the functions

SPDCALC_Handle SPDCALC_init(void *pMemory, const size_t numBytes)
{
    SPDCALC_Handle handle;

    if(numBytes < sizeof(SPDCALC_Obj))
    {
        return((SPDCALC_Handle)NULL);
    }

    // assign the handle
    handle = (SPDCALC_Handle)pMemory;

    return(handle);
} // end of SPDCALC_init() function

//------------------------------------------------------------------------------
void SPDCALC_reset(SPDCALC_Handle handle)
{
    SPDCALC_Obj *obj = (SPDCALC_Obj *)handle;

    obj->fbk = 0.0f;
    obj->Ui = 0.0f;

    return;
}

//------------------------------------------------------------------------------
void SPDCALC_setParams(SPDCALC_Handle handle)
{
    SPDCALC_Obj *obj = (SPDCALC_Obj *)handle;

    obj->thetaDelta = 0.3125e-3f;

    obj->Umax = 500.0f * MATH_TWO_PI;
    obj->Umin = -500.0f * MATH_TWO_PI;

    obj->Kp = 45.0f;
    obj->Ki = 50.0f * obj->thetaDelta;

    obj->Ui = 0.0f;

    return;
} // end of ESMO_run() function



void speedcal_pll_init(void)
{
	SPDCALC_setParams(&speedpll[HAL_MTR_1]);
	SPDCALC_setParams(&speedpll[HAL_MTR_2]);
}


//----------------------------------------------------------------

// end of file
